# import numpy as np
# import pandas as pd
# from scipy.optimize import curve_fit
# import matplotlib.pyplot as plt
#
# # 读取数据
# data = pd.read_csv('../Data/路径3.csv')
# x = data['x'].values
# y = data['y'].values
# z = data['z'].values
#
# # 定义更复杂的多项式模型
# def polynomial_model(X, a, b, c, d, e, f, g):
#     x, y = X
#     return a * x**2 + b * y**2 + c * x * y + d * x + e * y + f
#
# # 使用curve_fit进行拟合
# params, covariance = curve_fit(polynomial_model, (x, y), z)
#
# # 获取拟合参数
# a, b, c, d, e, f = params
#
# # 打印拟合参数
# print(f"拟合参数: a = {a}, b = {b}, c = {c}, d = {d}, e = {e}, f = {f}")
#
# # 计算拟合结果
# z_fit = polynomial_model((x, y), a, b, c, d, e, f)
#
# # 绘制原始数据和拟合曲线
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot(x, y, z, c = 'black', label = 'original')
# ax.plot(x, y, z_fit,c = 'blue',label = 'fixed')
# ax.set_xlabel('x')
# ax.set_ylabel('y')
# ax.set_zlabel('z')
# ax.legend()
# plt.show()

# import pandas as pd
# import numpy as np
# from scipy.optimize import curve_fit
# import matplotlib.pyplot as plt
#
# # 读取数据
# data = pd.read_csv('../Data/路径3 - Copy.csv')
# x = data['x'].values
# y = data['y'].values
# z = data['z'].values
#
# # 定义简化的二次多项式模型
# def quadratic_model(X, a, b, c, d, e):
#     x, y = X
#     return a * x**2 + b * y**2 + c * x + d * y + e
#
# # 提供初始参数猜测值
# initial_guess = [1, 1, 1, 1, 1]
#
# # 使用curve_fit进行拟合
# params, covariance = curve_fit(quadratic_model, (x, y), z, p0=initial_guess, maxfev=5000)
#
# # 获取拟合参数
# a, b, c, d, e = params
#
# # 打印拟合参数
# print(f"拟合参数: a = {a}, b = {b}, c = {c}, d = {d}, e = {e}")
#
# # 计算拟合结果
# z_fit = quadratic_model((x, y), a, b, c, d, e)
#
# # 绘制原始数据和拟合曲线
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot(x, y, z, c = 'black', label = 'original')
# ax.plot(x, y, z_fit,c = 'blue',label = 'fixed')
# ax.set_xlabel('x')
# ax.set_ylabel('y')
# ax.set_zlabel('z')
# ax.legend()
# plt.show()

# # 三次样条差值
# import numpy as np
# from scipy.interpolate import CubicSpline
# import matplotlib.pyplot as plt
#
# # 生成一些示例数据点
# x = np.array([1, 2, 3, 4, 5])
# y = np.array([2, 4, 1, 6, 3])
#
# # 创建三次样条插值对象
# cs = CubicSpline(x, y)
#
# # 生成用于绘制插值曲线的新的x值
# x_new = np.linspace(x[0], x[-1], 100)
# y_new = cs(x_new)
#
# # 绘制原始数据点和插值曲线
# plt.scatter(x, y, label='Original Points')
# plt.plot(x_new, y_new, label='Cubic Spline Interpolation')
# plt.xlabel('x')
# plt.ylabel('y')
# plt.legend()
# plt.show()

import matplotlib.pyplot as plt
import numpy as np
from scipy.interpolate import CubicSpline


def smooth_path_with_cubic_spline(path_points):
    """
    使用三次样条拟合对二维离散路径点进行平滑处理
    分段

    Args:
        path_points (list): 包含二维离散路径点的列表，每个元素为 (x, y) 形式的元组

    Returns:
        list: 经过三次样条拟合后的平滑路径点列表，每个元素同样为 (x, y) 形式的元组
    """
    x_coords = []
    y_coords = []

    # 提取x和y坐标到单独的列表
    for point in path_points:
        x_coords.append(point[0])
        y_coords.append(point[1])

    # 将坐标列表转换为numpy数组，方便后续计算
    x_coords = np.array(x_coords)
    y_coords = np.array(y_coords)

    # 创建三次样条插值函数
    cs = CubicSpline(x_coords, y_coords)

    # 生成插值后的x坐标
    x_new = np.linspace(x_coords[0], x_coords[-1], 150)

    # 计算插值后的y坐标
    y_new = cs(x_new)

    # 生成新的平滑路径点
    new_path_points = []
    for i in range(len(x_new)):
        x = x_new[i]
        y = y_new[i]
        new_path_points.append((x, y))

    return new_path_points


import random


def generate_random_path_points(num_points):
    """
    生成指定数量的随机二维离散路径点列表

    Args:
        num_points (int): 要生成的路径点数量

    Returns:
        list: 包含随机二维离散路径点的列表，每个元素为 (x, y) 形式的元组
    """
    path_points = []
    x_start, y_start = 0.0, 0.0

    for _ in range(num_points):
        x = x_start
        y = y_start
        x_step = random.uniform(2, 10)
        y_step = random.uniform(2, 10)
        path_points.append((x, y))
        if random.random() > 0.2:
            x_start += x_step
        else:
            x_start -= x_step
        y_start += y_step

        if x_start > 100 or y_start > 100:
            break
    return path_points


# import numpy as np
# import matplotlib.pyplot as plt


def bezier_curve(t, points):
    """
    计算贝塞尔曲线上给定参数t对应的点

    Args:
        t (float): 参数，取值范围为[0, 1]
        points (list): 包含控制点的列表，每个元素为 (x, y) 形式的元组

    Returns:
        tuple: 贝塞尔曲线上对应参数t的点的坐标 (x, y)
    """
    n = len(points) - 1
    curve_point = np.zeros(2)
    for i in range(len(points)):
        binomial_coefficient = np.math.factorial(n) / (np.math.factorial(i) * np.math.factorial(n - i))
        curve_point += binomial_coefficient * (1 - t) ** (n - i) * t ** i * points[i]
    return tuple(curve_point)


def fit_bezier_curve(path_points):
    """
    对给定的二维离散路径点列表进行贝塞尔曲线拟合

    Args:
        path_points (list): 包含二维离散路径点的列表，每个元素为 (x, y) 形式的元组

    Returns:
        list: 拟合后的贝塞尔曲线上的点的列表，每个元素为 (x, y) 形式的元组
    """
    t_values = np.linspace(0, 1, 100)
    bezier_curve_points = [bezier_curve(t, path_points) for t in t_values]
    return bezier_curve_points


# 示例用法
if __name__ == "__main__":
    # 生成一些示例的二维离散路径点
    path_points = [(1, 2), (3, 4), (5, 6), (7, 8)]

    # 进行贝塞尔曲线拟合
    fitted_bezier_curve_points = fit_bezier_curve(path_points)

    # 绘制原始路径点和拟合后的贝塞尔曲线
    plt.scatter([point[0] for point in path_points], [point[1] for point in path_points], label='Original Path Points')
    plt.plot([point[0] for point in fitted_bezier_curve_points], [point[1] for point in fitted_bezier_curve_points],
             label='Fitted Bezier Curve')
    plt.xlabel('X')
    plt.ylabel('Y')
    plt.legend()
    plt.show()

# 示例用法
# if __name__ == "__main__":
#     # 生成一些示例的二维离散路径点
#     path_points = generate_random_path_points(20)
#
#     # 进行三次样条拟合并获取平滑后的路径点
#     smoothed_path_points = smooth_path_with_cubic_spline(path_points)
#
#     # 绘制原始路径点和拟合后的平滑路径点
#     plt.scatter([point[0] for point in path_points], [point[1] for point in path_points],
#                 label = 'Original Path Points')
#     plt.plot([point[0] for point in path_points], [point[1] for point in path_points],
#              label = 'Original Path')
#     # plt.plot([point[0] for point in smoothed_path_points], [point[1] for point in smoothed_path_points], label='Smoothed Path')
#     plt.xlabel('X')
#     plt.ylabel('Y')
#     plt.legend()
#     plt.show()
